# coding=utf-8
# from Boiler import Boiler
from Bowl import Bowl
from DobotDllType import Pose
from time import sleep

import time
import traceback, sys

import serial

import modbus_tk
import modbus_tk.defines as cst
from modbus_tk import modbus_rtu

from Gripper import Gripper
from Track import Track
from Wrist import Wrist
from Arm import Arm
import DobotDllType as dType

logger = modbus_tk.utils.create_logger("console")


def get_tools_instance(tool_name):
    if tool_name is "Gripper":
        return Gripper()
    elif tool_name is "Track":
        return Track()
    elif tool_name is "Wrist":
        return Wrist()
    elif tool_name is "Arm":
        return Arm()
    elif tool_name is "Boiler":
        return Boiler()
    elif tool_name is "Bowl":
        return Bowl()
    else:
        return None


def do_action(tool_name, parameters, action):
    obj = get_tools_instance(tool_name)

    if obj:
        return obj.do_action(parameters, action)


def wait_action(tool_name, parameters):
    obj = get_tools_instance(tool_name)

    if obj:
        return obj.wait_action(parameters)


get_action_list = [
    # {"action": "Boiler", "params": {"on": True}},
    # {"action": "Bowl", "params": {"ready": True}},
    # {"action": "Wrist", "params": {"angle": 85, "duration": 2000}},  # 手腕恢复正常
    # {"action": "Arm", "params": {"CoordinateParams": [200.0, 200.0, 100.0, 100.0], "CommonParams": [20, 10],
    #                              "JointParams": [200, 200, 200, 200, 200, 200, 200, 200], "not_wait": True}},
    # {"action": "Track", "params": {"pos": 0, "wait_timeout": 20}},
    # {"action": "Gripper", "params": True},
    {"action": "Arm", "params": {"io": True, "pin": 2, "value": 1, "not_wait": True}},
    {"action": "Arm", "params": {"pwm": True, "pin": 4, "freq": 50, "duty_ratio": 2.85, "not_wait": True}}, # 0度
    {"action": "Arm", "params": {"x": 150, "y": 0, "z": -15}},
    {"action": "Arm", "params": {"x": 150, "y": 0, "z": -15}},
    {"action": "Arm", "params": {"x": 0, "y": 160, "z": -15}},
    {"action": "Arm", "params": {"x": 0, "y": 200, "z": -15, "wait_timeout": 2000}},
    {"action": "Arm", "params": {"x": 0, "y": 200, "z": -10}},
    # {"action": "Gripper", "params": False},
    # {"action": "Arm", "params": {"x": 0, "y": 240, "z": 30}},
    # {"action": "Arm", "params": {"x": 0, "y": 160, "z": 30}},
    {"action": "Arm", "params": {"x": 0, "y": -160, "z": 20}},
    # {"action": "Arm", "params": {"x": 0, "y": -144, "z": -40, "mode": dType.PTPMode.PTPJUMPXYZMode}},#1
    # {"action": "Arm",
    #  "params": {"CoordinateParams": [1, 1, 100, 100], "CommonParams": [20, 10],
    #             "JointParams": [50, 50, 50, 50, 50, 50, 50, 50], "not_wait": True}},
    {"action": "Arm", "params": {"x": 0, "y": -144, "z": -40}},  # 1
    # {"action": "Track", "params": {"pos": 0, "find_hold_index": True, "wait_timeout": 20}},  # 找到炉子孔的位置
    # {"action": "Track", "params": {"pos": 0, "wait_timeout": 20}},  # 找到炉子孔的位置
    [
        # {"action": "Wrist", "params": {"angle": 720, "duration": 4000}},
        {"action": "Arm", "params": {"pwm": True, "pin": 4, "freq": 50, "duty_ratio": 12.5, "not_wait": True}},
        {"action": "Arm", "params": {"x": -20, "y": -144, "z": -20}},
    ],
    {"action": "Arm", "params": {"x": -20, "y": -144, "z": -10}},
    {"action": "Arm", "params": {"x": -20, "y": -144, "z": -20}},
    {"action": "Arm", "params": {"x": -20, "y": -144, "z": -10}},
    {"action": "Arm", "params": {"x": -20, "y": -144, "z": -20}},

    {"action": "Arm", "params": {"x": -20, "y": -144, "z": -30}},


    {"action": "Arm", "params": {"pwm": True, "pin": 4, "freq": 50, "duty_ratio": 2.85, "wait_timeout": 4000}}, # 0度
    {"action": "Arm", "params": {"x": 150, "y": 0, "z": -15}},
    {"action": "Arm", "params": {"io": True, "pin": 2, "value": 0, "not_wait": True}},

    # {"action": "Wrist", "params": {"angle": 85, "duration": 2000}},  # 手腕恢复正常
    # {"action": "Track", "params": {"pos": 100, "wait_timeout": 10}},
    # {"action": "Arm", "params": {"x": -80, "y": -164, "z": -40}},
    # {"action": "Gripper", "params": True},
    # {"action": "Arm", "params": {"x": -80, "y": -164, "z": 10}},# 机械臂上升
]

pour_action_lists = [
    {"action": "Track", "params": {"pos": 0, "wait_timeout": 10}},
    {"action": "Wrist", "params": {"angle": 85, "duration": 4000}},
    {"action": "Arm", "params": {"x": 0, "y": -144, "z": -40 + 50}},
    {"action": "Arm", "params": {"x": 0, "y": -144, "z": -40}},  # 1
    {"action": "Arm", "params": {"x": 0, "y": -144 - 140, "z": -40}},  # 2 colander_pos
    {"action": "Gripper", "params": False},
    {"action": "Arm", "params": {"x": 0, "y": -144 - 140, "z": -40 + 150}},
    {"action": "Track", "params": {"pos": 3500, "wait_timeout": 20}},

    {"action": "Arm",
     "params": {"CoordinateParams": [1, 1, 100, 100], "CommonParams": [20, 10],
                "JointParams": [30, 30, 30, 30, 30, 30, 30, 30], "not_wait": True}},
    [
        {"action": "Wrist", "params": {"angle": 720, "duration": 4000}},
        {"action": "Arm", "params": {"x": 0 + 80, "y": -144 - 140 + 40, "z": -40 + 150 - 200}},
    ],
    [
        {"action": "Arm", "params": {"x": 0, "y": -144 - 140, "z": -40 + 150}},  # 2 colander_pos
        {"action": "Wrist", "params": {"angle": 85, "duration": 4000}},
    ],
    # {"action": "Track", "params": 0},
    {"action": "Track", "params": {"pos": 0, "find_hold_index": True, "wait_timeout": 20}},  # 找到炉子孔的位置
    {"action": "Arm", "params": {"x": 0, "y": -144 - 140, "z": -40}},  # 2 colander_pos
    {"action": "Gripper", "params": True},
    {"action": "Arm", "params": {"x": 0, "y": -144, "z": -40}},  # 1

    #      todo arm speed reset
]


# action_index = 0
# action_cmd = action_lists[action_index]

def exec_action_seq(action_list: list, action):
    interruption = False
    for action_cmd in action_list:
        # 如果是列表，则列表内的动作同时启动
        if isinstance(action_cmd, list):
            for a in action_cmd:
                action_name = a.get("action")
                if not do_action(action_name, a.get("params"), action):
                    interruption = True
                    break
            if interruption:
                break

            for a in action_cmd:
                action_name = a.get("action")
                if not wait_action(action_name, a.get("params")):
                    interruption = True
                    break
            if interruption:
                break
        # 如果是字典则运行单个动作
        elif isinstance(action_cmd, dict):
            action_name = action_cmd.get("action")
            if not do_action(action_name, action_cmd.get("params"), action):
                break
            if not wait_action(action_name, action_cmd.get("params")):
                break


def get_box_action(action):
    exec_action_seq(get_action_list, action)


def pour_action(action):
    exec_action_seq(pour_action_lists, action)


def stop_action():
    Track().interrupt(True)
    Wrist().interrupt(True)
